
template <uint8_t tInitialSize>
inline bool CDataStore<tInitialSize>::GetValue(uint16_t key, void* ptrData, uint8_t sizeData, uint8_t* ptrSizeRequired) const
{
	int offset = this->GetOffsetForKey(key);
	if (offset >= 0)
	{
		// the next byte gives us the size
		uint8_t actualSize = this->builtinStore[offset + 2];
		if (ptrSizeRequired != nullptr)
		{
			*ptrSizeRequired = actualSize;
		}

		if (ptrData != nullptr)
		{
			memcpy(ptrData, this->builtinStore + offset + 3, (std::min)(sizeData, actualSize));
		}

		return true;
	}

	return false;
	/*
	for (uint8_t i = 0; i < this->sizeUsed;)
	{
	if (*((uint16_t*)(this->builtinStore + i)) == key)
	{
	// the next byte gives us the size
	uint8_t actualSize = this->builtinStore[i + 2];
	if (ptrSizeRequired != nullptr)
	{
	*ptrSizeRequired = actualSize;
	}

	if (ptrData != nullptr)
	{
	memcpy(ptrData, this->builtinStore + i + 3, (std::min)(sizeData, actualSize));
	}

	return true;
	}

	// advance to next chunk
	i += this->builtinStore[i + 2];
	}

	return false;
	*/
}


template <uint8_t tInitialSize>
inline ITinyDataStore::AddValueRetCode CDataStore<tInitialSize>::AddOrSetValue(uint16_t key, void* ptrData, uint8_t sizeData)
{
	int offset = this->GetOffsetForKey(key);
	if (offset < 0)
	{
		// key is new!

		// TODO: check if it fits in
		if (sizeof(builtinStore) - this->sizeUsed < ((int)sizeData) + 3)
		{
			return ITinyDataStore::AddValueRetCode::Failure_CapacityExceeded;
		}

		*((uint16_t*)((&this->builtinStore[0]) + this->sizeUsed)) = key;

		this->builtinStore[this->sizeUsed + 2] = sizeData;
		memcpy(&(this->builtinStore[this->sizeUsed + 3]), ptrData, sizeData);
		this->sizeUsed += sizeData + 3;
		return ITinyDataStore::AddValueRetCode::Success_Added;// DataStoreStatusCode::OK_Added;
	}

	uint8_t sizeCurrent = this->builtinStore[offset + 2];
	if (sizeCurrent < sizeCurrent)
	{
		return ITinyDataStore::AddValueRetCode::Failure_ItemsLengthTooLarge;// DataStoreStatusCode::LengthMismatch;
	}

	memcpy(&(this->builtinStore[offset + 3]), ptrData, (std::min)(sizeData, sizeCurrent));
	return ITinyDataStore::AddValueRetCode::Success_Updated;// DataStoreStatusCode::OK_Updated;
}

template <uint8_t tInitialSize>
inline int CDataStore<tInitialSize>::GetOffsetForKey(uint16_t key) const
{
	for (int i = 0; i < this->sizeUsed;)
	{
		if (*((uint16_t*)(this->builtinStore + i)) == key)
		{
			return i;
		}

		// advance to next chunk
		i += 3 + this->builtinStore[i + 2];
	}

	return -1;
}